(* ::Package:: *)

(************************************************************************)
(* This file was generated automatically by the Mathematica front end.  *)
(* It contains Initialization cells from a Notebook file, which         *)
(* typically will have the same name as this file except ending in      *)
(* ".nb" instead of ".m".                                               *)
(*                                                                      *)
(* This file is intended to be loaded into the Mathematica kernel using *)
(* the package loading commands Get or Needs.  Doing so is equivalent   *)
(* to using the Evaluate Initialization Cells menu command in the front *)
(* end.                                                                 *)
(*                                                                      *)
(* DO NOT EDIT THIS FILE.  This entire file is regenerated              *)
(* automatically each time the parent Notebook file is saved in the     *)
(* Mathematica front end.  Any changes you make to this file will be    *)
(* overwritten.                                                         *)
(************************************************************************)



(* ::Section::Closed:: *)
(*Linkage geometry*)


(* ::Input:: *)
(*ClearAll["Global`*"];*)


(* ::Text:: *)
(*KT defined from output as a function of input*)


(* ::Input:: *)
(*Ldb[\[Theta]_]:=Sqrt[L2^2+L1^2-2 L1 L2 Cos[\[Theta]]]*)
(*\[CurlyPhi][\[Theta]_]:=ArcCos[(L3^2+L4^2 -Ldb[\[Theta]]^2)/(2 L3 L4)]*)


(* ::Input:: *)
(*FullSimplify[\[CurlyPhi]'[\[Theta]]]*)


(* ::Input:: *)
(*FullSimplify[\[CurlyPhi]'[\[Pi]/2]]*)


(* ::Input:: *)
(*ClearAll["Global`*"];*)


(* ::Text:: *)
(*Input angle as a function of output angle*)


(* ::Input:: *)
(*L[\[CurlyPhi]_]:= Sqrt[L3^2+L4^2-2 L3 L4 Cos[\[CurlyPhi]]]*)


(* ::Input:: *)
(*\[Theta][\[CurlyPhi]_]:=ArcCos[(L2^2+L1^2 -L[\[CurlyPhi]]^2)/(2 L1 L2)]*)


(* ::Text:: *)
(*KT is equal to the ratio of output angle change to input angle change *)


(* ::Input:: *)
(*KT[\[CurlyPhi]_]:=1/\[Theta]'[\[CurlyPhi]]*)


(* ::Input:: *)
(*FullSimplify[KT[\[CurlyPhi]]]*)


(* ::Input:: *)
(*FullSimplify[KT[\[Pi]/2]]*)


(* ::Input:: *)
(*p1 = Plot[\[Theta][\[CurlyPhi]]/.{L1->1,L2->.75,L3->.2,L4->1},{\[CurlyPhi],0,Pi},PlotStyle->RGBColor[1,0,0]];*)
(*p2= Plot[\[Theta][\[CurlyPhi]]/.{L1->1,L2->.75,L3->.1,L4->1},{\[CurlyPhi],0,Pi},PlotStyle->RGBColor[0,1,0]];*)
(*Show[p1,p2,DisplayFunction->$DisplayFunction]*)


(* ::Input:: *)
(*p1=Plot[KT[\[CurlyPhi]]/.{L1->1,L2->.75,L3->.2,L4->1},{\[CurlyPhi],0,Pi},PlotStyle->RGBColor[1,0,0]];*)
(*p2=Plot[KT[\[CurlyPhi]]/.{L1->1,L2->.75,L3->.1,L4->1},{\[CurlyPhi],0,Pi},PlotStyle->RGBColor[0,1,0]];*)
(*Show[p1,p2,DisplayFunction->$DisplayFunction]*)


(* ::Section::Closed:: *)
(*Heuristic model*)


(* ::Input:: *)
(*ClearMech[];ClearAll["Global`*"]; Needs["MechanicalSystems`Modeler2D`"];Off[General::"spell"];Off[General::"spelll"];*)


(* ::Input:: *)
(*pVals = {m->.003,k->15,hIn->.02,hOut ->.04,\[Theta]Initial->Pi};*)


(* ::Input:: *)
(*\[Theta][t_]=\[Theta][t]/.DSolve[{k hIn^2 \[Theta][t]+hOut^2 m  \[Theta]''[t]==0,\[Theta]'[0]==0,\[Theta][0]==\[Theta]Initial},\[Theta][t],t][[1]]*)


(* ::Input:: *)
(*\[Theta]'[t]*)


(* ::Input:: *)
(*FullSimplify[\[Theta]''[t]]*)


(* ::Input:: *)
(*FullSimplify[m \[Theta]'[t]]*)


(* ::Input:: *)
(*\[Theta][t]/.pVals/.{t->1}*)


(* ::Input:: *)
(*period=2 * Pi/(hIn Sqrt[k] )/(hOut Sqrt[m])/.pVals*)


(* ::Input:: *)
(*Plot[\[Theta][t]/.pVals,{t,0,.05}]*)


(* ::Input:: *)
(*Plot[\[Theta]'[t]/.pVals,{t,0,.05}]*)


(* ::Section::Closed:: *)
(*Heuristic model 2*)


(* ::Input:: *)
(*ClearMech[];ClearAll["Global`*"]; Needs["MechanicalSystems`Modeler2D`"];Off[General::"spell"];Off[General::"spelll"];*)


(* ::Input:: *)
(*pVals = {m->.003,k->15,hIn->.02,hOut ->.04,\[Theta]Initial->Pi,B->1};*)


(* ::Input:: *)
(*\[Theta][t_]=\[Theta][t]/.DSolve[{k ( hIn-B \[Theta][t])^2 \[Theta][t]+hOut^2 m  \[Theta]''[t]==0,\[Theta]'[0]==0,\[Theta][0]==\[Theta]Initial},\[Theta][t],t][[1]];*)


(* ::Input:: *)
(*Plot[\[Theta]'[t]/.pVals,{t,0,.05}]*)


(* ::Section::Closed:: *)
(*Heuristic model with linear change in in-lever*)


(* ::Input:: *)
(*ClearMech[];ClearAll["Global`*"]; Needs["MechanicalSystems`Modeler2D`"];Off[General::"spell"];Off[General::"spelll"];*)


(* ::Input:: *)
(*numLs = 5;*)
(*pVals = {m->1,k->1,hIn->.5,hOut ->1,\[Theta]Initial->Pi,B->.1};*)
(*tSim = 10;*)


(* ::Input:: *)
(*\[Theta]N[t_]=\[Theta][t]/.NDSolve[{k ( hIn-B t)^2 \[Theta][t]+hOut^2 m  \[Theta]''[t]==0,\[Theta]'[0]==0,\[Theta][0]==\[Theta]Initial}*)
(*/.pVals,\[Theta],{t,0,tSim}][[1]];*)


(* ::Input:: *)
(*\[Theta][t_]=\[Theta][t]/.DSolve[{k hIn^2 \[Theta][t]+hOut^2 m  \[Theta]''[t]==0,\[Theta]'[0]==0,\[Theta][0]==\[Theta]Initial},\[Theta][t],t][[1]];*)


(* ::Input:: *)
(*Plot[{\[Theta][t]/.pVals,\[Theta]N[t]},{t,0,tSim}]*)


(* ::Input:: *)
(*Plot[{\[Theta]'[t]/.pVals,\[Theta]N'[t]},{t,0,tSim}]*)


(* ::Input:: *)
(*Plot[ (hIn-B t)/.pVals,{t,0,tSim}]*)


(* ::Section::Closed:: *)
(*Heuristic model with linear change in out-lever*)


(* ::Input:: *)
(*ClearMech[];ClearAll["Global`*"]; Needs["MechanicalSystems`Modeler2D`"];Off[General::"spell"];Off[General::"spelll"];*)


(* ::Input:: *)
(*numLs = 5;*)
(*pVals = {m->1,k->1,hIn->.5,hOut ->1,\[Theta]Initial->Pi,B->1*.1};*)
(*tSim = 10;*)


(* ::Input:: *)
(*\[Theta]N[t_]=\[Theta][t]/.NDSolve[{k hIn^2 \[Theta][t]+(hOut+B t)^2 m  \[Theta]''[t]==0,\[Theta]'[0]==0,\[Theta][0]==\[Theta]Initial}*)
(*/.pVals,\[Theta],{t,0,tSim}][[1]];*)


(* ::Input:: *)
(*\[Theta][t_]=\[Theta][t]/.DSolve[{k hIn^2 \[Theta][t]+(hOut+B t)^2 m  \[Theta]''[t]==0,\[Theta]'[0]==0,\[Theta][0]==\[Theta]Initial},\[Theta][t],t][[1]];*)


(* ::Input:: *)
(*v[t_]=\[Theta]'[t] (hOut+B t);*)


(* ::Input:: *)
(*Plot[{\[Theta][t]/.pVals,\[Theta]N[t]},{t,0,tSim}]*)


(* ::Input:: *)
(*Plot[{\[Theta]'[t]/.pVals,\[Theta]N'[t]},{t,0,tSim}]*)


(* ::Input:: *)
(*FullSimplify[\[Theta]'[t]]*)


(* ::Input:: *)
(*N[(\[Theta]'[t] )/.pVals/.{t->3.14}]*)


(* ::Output:: *)
(*-1.5708*)


(* ::Output:: *)
(*-1.34632-2.77556*10^-17 \[ImaginaryI]*)


(* ::Input:: *)
(*Plot[v[t]/.pVals,{t,0,tSim}]*)


(* ::Input:: *)
(*Plot[ (hOut+B t)/.pVals,{t,0,tSim}]*)


(* ::Section::Closed:: *)
(*Heuristic model with nonlinear in-lever (unfinished)*)


(* ::Input:: *)
(*ClearMech[];ClearAll["Global`*"]; Needs["MechanicalSystems`Modeler2D`"];Off[General::"spell"];Off[General::"spelll"];*)


(* ::Input:: *)
(*numLs = 5;*)
(*pVals = {m->.003,k->15,hOut->.04,\[Theta]Initial->Pi};*)
(*lVals = {L1->1,L2->.75,L4->1};*)


(* ::Input:: *)
(*KT[\[CurlyPhi]_]:=(L1 L2 Sqrt[1-(L1^2+L2^2-L3^2-L4^2+2 L3 L4 Cos[\[CurlyPhi]])^2/(4 L1^2 L2^2)] Csc[\[CurlyPhi]])/(L3 L4)*)


(* ::Input:: *)
(*KTval[\[Theta]_]=KT[ .9\[Theta]+Pi/50]/.lVals;*)


(* ::Input:: *)
(*L3vals = Range[.2*.5,.2*1.5,.2 (1.5-0.5)/numLs];*)
(*dataList={};*)


(* ::Input:: *)
(*For [i=1,i<numLs+1,*)
(*	L3=L3vals[[i]];*)
(*\[Theta][t_]=\[Theta][t]/.NDSolve[{k (hOut/KTval[\[Theta][t]])^2 \[Theta][t]+hOut^2 m  \[Theta]''[t]==0,\[Theta]'[0]==0,\[Theta][0]==\[Theta]Initial}*)
(*/.pVals,\[Theta],{t,0,2}][[1]];*)
(*dataList=Append[dataList,{Log[KT[Pi/2]/.lVals,10],Log[Max[\[Theta]'[t]/.{t->Table[t,{t,0,2,2/1000}]}],10]}];*)
(*Clear[\[Theta]];*)
(*i++]*)
(**)


(* ::Input:: *)
(*line=Fit[dataList,{1,x},x]*)


(* ::Input:: *)
(*Show[ListPlot[dataList,PlotStyle->Red],Plot[line,{x,1.,2.4}]]*)


(* ::Section::Closed:: *)
(*Gear model *)


(* ::Input:: *)
(*ClearMech[];ClearAll["Global`*"]; Needs["MechanicalSystems`Modeler2D`"];Off[General::"spell"];Off[General::"spelll"];*)


(* ::Input:: *)
(*numLs = 5;*)
(*pVals = {m->1,k->1,hIn->.5,hOut ->1,\[Theta]0->Pi/4,B->1*.1};*)
(*tSim = 10;*)


(* ::Input:: *)
(*\[Theta][t_]=\[Theta][t]/.FullSimplify[DSolve[{m hOut^2  gr^2 \[Theta]''[t]+k \[Theta][t]-k \[Theta]R==0,\[Theta]'[0]==0,\[Theta][0]==\[Theta]0},\[Theta][t],t]][[1]]*)


(* ::Input:: *)
(*\[Theta]'[t]*)


(* ::Input:: *)
(*v= \[Theta]'[t] gr hOut*)


(* ::Section::Closed:: *)
(*Gear model with drag - no solution*)


(* ::Input:: *)
(*ClearMech[];ClearAll["Global`*"]; Needs["MechanicalSystems`Modeler2D`"];Off[General::"spell"];Off[General::"spelll"];*)


(* ::Input:: *)
(*numLs = 5;*)
(*pVals = {m->1,k->1,hIn->.5,hOut ->1,\[Theta]0->Pi/4,B->1*.1};*)
(*tSim = 10;*)


(* ::Input::Closed:: *)
(*\[Theta][t_]=\[Theta][t]/.FullSimplify[DSolve[{m hOut^2  gr^2 \[Theta]''[t]+Cd (\[Theta]'[t])^2 +k \[Theta][t]-k \[Theta]R==0,\[Theta]'[0]==0,\[Theta][0]==\[Theta]0},\[Theta][t],t]][[1]]*)


(* ::Message:: *)
(*$RecursionLimit::"reclim": "Recursion depth of \!\(256\) exceeded. \!\(\*ButtonBox[\"\[RightSkeleton]\", ButtonStyle->\"Link\", ButtonFrame->None, ButtonData:>\"paclet:ref/message/$RecursionLimit/reclim\", ButtonNote -> \"$RecursionLimit::reclim\"]\)"*)


(* ::Input:: *)
(*\[Theta]'[t]*)


(* ::Input:: *)
(*v= \[Theta]'[t] gr hOut*)


(* ::Section::Closed:: *)
(*Import parameter values (instead of defining)*)


(* ::Subsection:: *)
(*Set parameters*)


(* ::Input:: *)
(*KillMech[];ClearMech[];ClearAll["Global`*"]; Needs["MechanicalSystems`Modeler2D`"];Off[General::"spell"];*)


(* ::Input:: *)
(*currPath = Import["/Volumes/Docs/Projects/Patek_project/pod_model/sims/curr_path.txt",{"Text","Lines"}];*)


(* ::Input:: *)
(*currPath*)


(* ::Input:: *)
(*inParams = Import[StringJoin[currPath, "/input_params.mat"]][[1]];*)


(* ::Input:: *)
(*tEvalIn = Import[StringJoin[currPath, "/eval_time.mat"]][[1]];*)


(* ::Input:: *)
(*L1In = inParams[[1]];*)
(*L2In=inParams[[2]];*)
(*L3In=inParams[[3]];*)
(*L4In=inParams[[4]];*)
(**)
(*EXLocalIn = inParams[[5]];*)
(*EYLocalIn=inParams[[6]];*)
(*FXLocalIn=inParams[[7]];*)
(*FYLocalIn=inParams[[8]];*)
(**)
(*thetaStartIn=inParams[[9]];*)
(*dMassIn=inParams[[10]];*)
(*dIIn=inParams[[11]];*)
(*kSpringIn=inParams[[12]];*)
(*thetaRestIn=inParams[[13]];*)
(**)
(*simDurationIn=inParams[[14]];*)
(*dAIn=inParams[[15]];*)
(*rhoIn=inParams[[16]];*)
(*CdIn=inParams[[17]];*)
(*waterIIn = inParams[[18]];*)
(*mxErrIn = inParams[[19]];*)


(* ::Input:: *)
(*Clear[inParams]*)


(* ::Section:: *)
(*Define parameter values (instead of importing)*)


(* ::Input:: *)
(*ClearMech[];ClearAll["Global`*"]; Needs["MechanicalSystems`Modeler2D`"];Off[General::"spell"];Off[General::"spelll"];*)


(* ::Input:: *)
(*(* Linakage coordinates *)*)
(*L1In=6 *10^-3;*)
(*L2In=3.75 *10^-3;*)
(*L3In=.425*10^-3;*)
(*L4In=6.7 *10^-3;*)
(*L5In=6.1* 10^-3;*)
(*dacLenIn = 9.63*10^-3;*)


(* ::Input:: *)
(*(* Distance between points A&F,determined by propodus thickness *)*)
(*hAFIn = 1 10^-3;*)


(* ::Input:: *)
(*(* Initial input angle (rad) *)*)
(*thetaStartIn = 1.3774;*)


(* ::Input:: *)
(*(* Dactyl mass (kg) *)*)
(*dMassIn = 4.53 *10^-4 ;*)


(* ::Input:: *)
(*(* Dactyl I (kg m^2) *)*)
(*dIIn= 8.16 *10^-8 ;*)


(* ::Input:: *)
(*(* Water I (kg m^2) *)*)
(*waterIIn = 1*10^-8;*)


(* ::Input:: *)
(*(* 3rd moment of area of dactyl *)*)
(*dAIn=1.5 * 10^-11;*)


(* ::Input:: *)
(*(* Drag coefficient of elliptical dactyl element *)*)
(*DIn = 0.07;*)


(* ::Input:: *)
(*(* Torsion spring at mV joint (Nm/rad) *)*)
(*kSpringIn = (60*10^3) (3.18 *10^-3)^2;   *)


(* ::Input:: *)
(*(* Resting position for torsion spring *)*)
(*thetaRestIn = 1.6088;*)
(**)
(*gammaIn = 60*Pi/180;*)


(* ::Input:: *)
(*(* Density of water (kg/m^3) *)*)
(*rhoIn=998;*)


(* ::Input:: *)
(*(* Duration of simulation (s) *)*)
(*simDurationIn = 6*10^-3;*)


(* ::Input:: *)
(*(* Time values to evaluate results *)*)
(*tEvalIn = Table[N[i],{i,0,simDurationIn,simDurationIn/1000}];*)


(* ::Input:: *)
(*(* Precision of simulation *)*)
(*mxErrIn = 10^-5;*)


(* ::Input:: *)
(*(* Designate current path *)*)
(*currPath="/Volumes/Docs/Projects/Patek_project/sims";*)


(* ::Section:: *)
(*Phenomeological model *)


(* ::Subsection::Closed:: *)
(*Rescale input parameter values*)


(* ::Text:: *)
(**)
(*Scale Factors (Mutliplers of all parameters -- helps with numerical instabilities)*)


(* ::Input:: *)
(*sL = 1 / L1In;*)
(*sM = 1/dMassIn;*)
(*sT=10^3;*)


(* ::Input:: *)
(*(* Apply scale factors to parameters *)*)
(*dMass = dMassIn sM;*)
(*dI= dIIn sM sL^2;*)
(*waterI=waterIIn sM sL^2;*)
(**)
(*L1=L1In sL;*)
(*L2=L2In sL;*)
(*L3=L3In sL;*)
(*L4=L4In  sL;*)
(*L5=L5In sL;*)
(*dacLen = dacLenIn sL;*)
(**)
(*(*EXLocal = EXLocalIn sL;*)
(*EYLocal = EYLocalIn sL;*)
(*FXLocal = FXLocalIn sL;*)
(*FYLocal = FYLocalIn sL;*)*)
(*hAF = hAFIn sL;*)
(*thetaStart    = thetaStartIn;*)
(*gamma = gammaIn;*)
(**)
(*kSpring = kSpringIn (sM sL^2/sT^2);*)
(*thetaRest= thetaRestIn;*)
(*dA = dAIn * sL^5;*)
(*rho = rhoIn * sM / sL^3;*)
(*DrgIdx =0* DIn;*)
(**)
(*simDuration = simDurationIn * sT;*)
(*tEval = tEvalIn*sT;*)
(**)
(*mxErr = mxErrIn;*)


(* ::Input:: *)
(*Clear[dMassIn, dIIn,L1In,L2In,L3In,L4In,L5In,kSpringIn,TmaxIn,hAFIn,thetaIn,simDurationIn,tEvalIn,simDurationIn,tEvalIn,dAIn,rhoIn,CdIn,waterIIn,mxErrIn,LCOMIn]*)


(* ::Subsection::Closed:: *)
(*Calculated parameter values, define model inputs*)


(* ::Text:: *)
(**)
(**)
(*Angle, \[Psi], btwn L1 & L4:*)


(* ::Input:: *)
(*(* Distance btwn B & D *)*)
(*hBD=Sqrt[L1^2+L2^2-2 L1 L2 Cos[thetaStart]];*)
(**)
(*(* This angle btwn 1 & 4 *)*)
(*si=ArcCos[(hBD^2+L1^2-L2^2)/(2 hBD L1)]+ArcCos[(hBD^2+L4^2-L3^2)/(2 hBD L4)];*)
(**)
(*(* Distance btwn B & F: *)*)
(*hBF=Sqrt[hAF^2+L2^2+2 hAF L2 Cos[thetaStart]];*)
(**)


(* ::Input:: *)
(*ArcCos[(hBD^2+L4^2-L3^2)/(2 hBD L4)]*)


(* ::Text:: *)
(**)
(**)
(*Check that the geometry is possible (both should be 'True')*)


(* ::Input:: *)
(*L4  < ( L3 + hBD)*)


(* ::Input:: *)
(*L4  > (  hBD-L3)*)


(* ::Input:: *)
(*L4/sL*)


(* ::Input:: *)
(*L1^2+L2^2-2 L1 L2 Cos[thetaStart]*)


(* ::Input:: *)
(*L1*)


(* ::Input:: *)
(*si*)


(* ::Text:: *)
(**)
(**)
(*Points on the appendage*)


(* ::Input:: *)
(*AX=0;*)
(*AY= 0;*)
(*BX=L2 Sin[thetaStart];*)
(*BY=L2 Cos[thetaStart];*)
(*CX=L4 Sin[si];*)
(*CY=L1-L4 Cos[si];*)
(*DX=0;*)
(*DY=L1;*)
(*FX = 0;*)
(*FY=-hAF;*)
(*GX=FX-L5 Sin[gamma];*)
(*GY=FY+L5 Cos[gamma];*)
(**)
(*(*EX =BX+ EXLocal;*)
(*EY=BY - EYLocal;*)
(*FX=BX- FXLocal;*)
(*FY=EY-FYLocal;*)*)


(* ::Input:: *)
(*Clear[si,hAF,hBF]*)


(* ::Input:: *)
(*(* Check distances *)*)
(*Sqrt[(BX-CX)^2+(BY-CY)^2]*)
(*L3*)


(* ::Input:: *)
(*si*)


(* ::Text:: *)
(**)
(**)
(*Body addresses*)


(* ::Input:: *)
(*ground = 1; mV = 2;carpus = 3;dactyl = 4;*)


(* ::Text:: *)
(**)
(**)
(*Display initial geometry*)


(* ::Input:: *)
(*offset = .09 L1;*)
(*Show[*)
(*ListPlot[{*)
(*{{AX,AY},{BX,BY}},*)
(*{{CX,CY},{DX,DY}},*)
(*{{BX,BY},{CX,CY},{BX,BY}},*)
(*{{GX,GY},{FX,FY}}*)
(*},*)
(*PlotMarkers->Automatic,Joined->True,*)
(*Frame->True,AspectRatio->Automatic,*)
(*PlotLabel->"Initial geometry",*)
(*PlotRange->{{-1,1},{-1,1}}],*)
(*Graphics[*)
(*{Text["A",{AX-offset,AY+offset}],Text["B",{BX-offset,BY+offset}],*)
(*Text["C",{CX+offset,CY+offset}],Text["D",{DX-offset,DY+offset}],*)
(*Text["F",{FX-offset,FY+offset}],*)
(*Text["G",{GX-offset,GY+offset}],*)
(*Text["L1",{Mean[{AX,DX}]-offset,Mean[{AY,DY}]}],*)
(*Text["L2",{Mean[{AX,BX}]-offset,Mean[{AY,BY}]}+offset],*)
(*Text["L3",{Mean[{CX,BX}]-offset,Mean[{CY,BY}]}+offset/2],*)
(*Text["L4",{Mean[{DX,CX}]-offset,Mean[{DY,CY}]}+offset]*)
(*}]*)
(*]*)


(* ::Subsection::Closed:: *)
(*Define Bodies*)


(* ::Input:: *)
(*bd[ground] = Body[ground,*)
(*PointList->{*)
(*(*P1*){DX,DY}},*)
(*InitialGuess->{*)
(*{0,0},0}*)
(*];*)


(* ::Input:: *)
(*bd[mV] = Body[mV,*)
(*PointList->{*)
(*(*P1*){BX,BY}},*)
(*Centroid->{0,0},*)
(*InitialGuess->{*)
(*{0,0},0}*)
(*];*)


(* ::Input:: *)
(*bd[carpus]=Body[carpus,*)
(*PointList->{(*P1*){CX-BX,CY-BY},*)
(*                       (*P2*){FX-BX,FY-BY},*)
(*                       (*P3*){GX-BX,GY-BY}},*)
(*Mass->dMass,Inertia->dI+waterI,*)
(*InitialGuess->{{BX,BY},0}];*)


(* ::Text:: *)
(*bd[dactyl] = Body[dactyl, PointList -> {(*P1*){GX - FX, GY - FY}}, Mass -> dMass, Inertia -> dI + waterI, InitialGuess -> {{FX, FY}, 0}];*)


(* ::Input:: *)
(*SetBodies[bd[ground],bd[mV],bd[carpus]];*)


(* ::Subsection::Closed:: *)
(*Define constraints*)


(* ::Text:: *)
(**)
(*Pin joint between ground and mV:*)


(* ::Input:: *)
(*cs[1]= Revolute2[1,Point[ground,0],Point[mV,0]];*)


(* ::Text:: *)
(**)
(*Initially fix position of joint at mV origin (removed later):*)


(* ::Input:: *)
(*cs[2] = RotationLock1[2,  mV,0];*)


(* ::Text:: *)
(**)
(*Pin joint between carpus and mV:*)


(* ::Input:: *)
(*cs[3] = Revolute2[3, Point[carpus, 0], Point[mV, 1]];*)


(* ::Text:: *)
(**)
(*Fix distance between ground and top of carpus:*)


(* ::Input:: *)
(*cs[4] = RelativeDistance1[4, Point[carpus, 1], Point[ground, 1], L4];*)


(* ::Text:: *)
(**)
(*Pin btwn carpus and dactyl*)


(* ::Text:: *)
(*cs[5] = Revolute2[5, Point[dactyl, 0], Point[carpus, 2]];*)


(* ::Text:: *)
(**)
(*Lock rotation btwn carpus & dactyl*)


(* ::Text:: *)
(*cs[6] = RotationLock1[6, carpus, dactyl, 0];*)


(* ::Input:: *)
(*SetConstraints[cs[1], cs[2], cs[3], cs[4]];*)


(* ::Input:: *)
(*Print["Check system after constraints:" Evaluate[CheckSystem[]]]*)


(* ::Subsection:: *)
(*Evaluate kinematic model*)


(* ::Text:: *)
(*SetParameters[{*)
(*   (*Torsion spring stiffness*)  theta -> \[Theta]*)
(*   }];*)


(* ::Input:: *)
(*graph=Graphics[{*)
(*(*ground*){RGBColor[0,0,1],Bar[Axis[ground,0,1],10^-4 sL,10^-4 sL]},*)
(*(*mV*){RGBColor[0,.5,.5],Bar[Line[mV,0,1],10^-4 sL,10^-4 sL]},*)
(*(*carpus1*){RGBColor[1,0,0],Bar[Line[carpus,0,1],10^-4 sL,10^-4 sL]},*)
(*(*carpus2*){RGBColor[1,0,0],Bar[Line[carpus,0,2],10^-4 sL,10^-4 sL]},*)
(*(*carpus3*){RGBColor[1,0,0],Vertex[carpus,{0,3}]}*)
(*},*)
(*Frame->True,*)
(*AspectRatio->Automatic,*)
(*GridLines->Automatic,*)
(*PlotRange->{{-1,1.5},{-2,1}}*)
(*];*)
(**)
(*Show[graph/.SolveMech[]]*)


(* ::Input:: *)
(*Evaluate[{Angle[mV,1]}/.SolveMech[]] (180/\[Pi])*)


(* ::Subsection:: *)
(*Define Loads*)


(* ::Text:: *)
(**)
(*Moment created by the spring*)


(* ::Text:: *)
(*springMoment = kSpring  ( Pi/2 - Angle[mV, 1] - thetaRest);*)


(* ::Input:: *)
(*springMoment=kSpring (Pi/2-Angle[mV,1]-thetaRest);*)
(*dragMoment=(-0.5*rho*(Velocity[carpus][[3]])^2*(dacLen^5)*DrgIdx) (Velocity[carpus][[3]]/Abs[10^-20+Velocity[carpus][[3]]]);*)


(* ::Input:: *)
(*ld[1]=Moment[mV,springMoment];*)


(* ::Text:: *)
(**)
(*Drag on dactyl*)


(* ::Text:: *)
(*ld[2] = Force[dactyl, Axis[Point[dactyl, 1], -Velocity[dactyl, 1]^2], 5.0, Magnitude -> Relative];*)


(* ::Input:: *)
(*ld[2]=Moment[carpus,dragMoment];*)


(* ::Input:: *)
(*ld[3]=Gravity[{0,-1},0];*)


(* ::Input:: *)
(*SetLoads[ld[1],ld[2]]*)


(* ::Input:: *)
(*Print["Check system after loads:" Evaluate[CheckSystem[]]]*)


(* ::Subsection:: *)
(*Find solution*)


(* ::Text:: *)
(*Solution with the 4-bar linkage constrained from moving:*)


(* ::Text:: *)
(*Remove constraint 2 (fixed angle of the mV)  & define initial conditions*)


(* ::Input:: *)
(*fsys = SetFree[2,{Solution->Dynamic,*)
(*InitialCondition->{T->0,\[CapitalTheta]2d->0,\[CapitalTheta]3d->0,\[CapitalTheta]4d->0,X2d->0,Y2d->0,X3d->0,Y3d->0,X4d->0,Y4d->0}}];*)


(* ::Text:: *)
(*SolveFree[fsys]*)


(* ::Input:: *)
(*(* Default solver *)*)
(*sol = SolveFree[fsys, simDuration, MakeRules -> {Location, Velocity, Acceleration},*)
(*MaxError->mxErr];*)


(* ::Text:: *)
(*(* High precision solver *)*)
(*sol = SolveFree[fsys, simDuration, MakeRules -> {Location, Velocity, Acceleration},*)
(*   ConstraintCorrection -> True,*)
(*   MaxError -> 10^-7,*)
(*   Method -> Corrector,*)
(*   FitDegree -> Quadratic,*)
(*   MaxSteps -> 5000];*)


(* ::Subsection::Closed:: *)
(*Simulation diagnostics (for debugging)*)


(* ::Input:: *)
(*Constraints[1]/.sol*)
(*Constraints[3]/.sol*)
(*Constraints[4]/.sol*)


(* ::Input:: *)
(*StepMech[]*)


(* ::Input:: *)
(*Loads[dactyl,Coordinates->Global][[2]]*)


(* ::Input:: *)
(*(Loads[carpus,Coordinates->Global])/.sol/.{T->tEval}*)


(* ::Subsection:: *)
(*Draw bodies*)


graph = Graphics[{
{RGBColor[0, 0, 1], Bar[Line[ground, 0, 1], sL/10^4, sL/10^4]}, 
{RGBColor[0, 0.5, 0.5], Bar[Line[mV, 0, 1], sL/10^4, sL/10^4]}, 
(*carpus1*){RGBColor[1,0,0],Bar[Line[carpus,0,1],10^-4 sL,10^-4 sL]},
(*carpus2*){RGBColor[1,0,0],Bar[Line[carpus,0,2],10^-4 sL,10^-4 sL]},
(*carpus3*){RGBColor[1,0,0],Vertex[carpus,{0,3}]}}, 
Frame -> True, AspectRatio -> Automatic, GridLines -> Automatic]; 


(* ::Input:: *)
(*EY*)


(* ::Input:: *)
(*Show[graph/.(sol/.{T->0*simDuration})]*)


(* ::Text:: *)
(*numPlot = 10; GraphicsGrid[{graph /. (sol /. Table[{T -> i}, {i, 0, simDuration, simDuration/numPlot}] )}]*)


(* ::Input:: *)
(*Animate[Show[graph /. (sol /. {T -> i})], {i, 0, simDuration, .1}]*)


(* ::Subsection:: *)
(*Graph results*)


(* ::Input:: *)
(*Plot[{(springMoment/.sol),T*0},{T,0,simDuration}]*)


(* ::Input:: *)
(*(180/Pi) Tmax/kSpring*)


(* ::Input:: *)
(*180/Pi thetaRest*)


(* ::Input:: *)
(*Plot[(180/Pi)(Angle[mV,1])/.sol,{T,0,simDuration}]*)


(* ::Input:: *)
(*Plot[(180/Pi)(Angle[carpus,1])/.sol,{T,0,simDuration}]*)


(* ::Input:: *)
(*Plot[Velocity[carpus][[3]]/.sol,{T,0,simDuration}]*)


(* ::Input:: *)
(*Plot[springMoment/.sol,{T,0,simDuration}]*)


(* ::Input:: *)
(*Plot[BodyEnergy[carpus]/.sol,{T,0,simDuration}]*)


(* ::Input:: *)
(* *)


(* ::Subsection:: *)
(*Energetics*)


(* ::Text:: *)
(*springEnergy[T_] := -((Angle[mV, 1] - thetaRest)/Abs[(Angle[mV, 1] - thetaRest)]) 0.5 kSpring (Angle[mV, 1] - thetaRest)^2 ;*)


(* ::Input:: *)
(*springEnergy=Abs[0.5springMoment (Pi/2-Angle[mV,1]-thetaRest)/.sol];*)


(* ::Input:: *)
(*dacSpeed=Sqrt[*)
(*(Velocity[carpus,3][[1]])^2+(Velocity[carpus,3][[2]])^2]/.sol;*)


(* ::Text:: *)
(*dragEnergy[ti_] := NIntegrate[totDrag[t], {t, 0, ti}];*)


(* ::Text:: *)
(*kineEnergy =(*Rotation *)(0.5 (dI + waterI) Velocity[carpus][[3]]^2  +*)
(*             (* Translation *) 0.5 dMass dacSpeed^2) /. sol;*)


(* ::Input:: *)
(*kineEnergy=(*Rotation *)(0.5 (dI+waterI) Velocity[carpus][[3]]^2 ) /.sol;*)


(* ::Input:: *)
(*kineEnergyMat = BodyEnergy[carpus]/.sol;*)


(* ::Input:: *)
(*kineEnergy/.{T->2}*)


(* ::Input:: *)
(*totEnergy = springEnergy  + kineEnergy;*)


(* ::Input:: *)
(*ListPlot[Table[{((180/Pi) (Pi/2-Angle[mV,1] -thetaRest)) /. sol, springMoment /. sol}, {T, simDuration/100, simDuration - simDuration/100, simDuration/100}]]*)


(* ::Input:: *)
(*kinePlot=Plot[kineEnergy,{T,0,simDuration},DisplayFunction->Identity,PlotStyle->RGBColor[1, 0,0]];*)
(*kinePlotMat=Plot[kineEnergyMat,{T,0,simDuration},DisplayFunction->Identity,PlotStyle->RGBColor[0, 1,0]];*)
(*springPlot=Plot[springEnergy,{T,0,simDuration},DisplayFunction->Identity,PlotStyle->{RGBColor[0, 0,1]}];*)
(*totPlot=Plot[totEnergy,{T,0,simDuration},DisplayFunction->Identity,PlotStyle->{RGBColor[0, 0,0]}];*)
(**)
(*Show[kinePlotMat,kinePlot,DisplayFunction->$DisplayFunction,PlotRange->{{0,simDuration},{0,1.1}}]*)


(* ::Input:: *)
(*Show[totPlot,DisplayFunction->$DisplayFunction]*)


(* ::Input:: *)
(*Show[springPlot,DisplayFunction->$DisplayFunction]*)


(* ::Input:: *)
(*kinePlot=Plot[kineEnergy[All]/.sol,{T,0,simDuration},DisplayFunction->Identity,PlotStyle->RGBColor[1, 0,0]];*)
(*springPlot=Plot[springEnergy[T]/.sol,{T,0,simDuration},DisplayFunction->Identity,PlotStyle->{RGBColor[0, 0,1],Dashed}];*)
(*totPlot=Plot[(kineEnergy[T]+springEnergy[T])/.sol,{T,0,simDuration},DisplayFunction->Identity,PlotStyle->{RGBColor[0, 0,0]}];*)
(*Show[springPlot,kinePlot,totPlot,DisplayFunction->$DisplayFunction]*)


(* ::Subsection:: *)
(*Export data*)


(* ::Input:: *)
(*(* P1 on carpus *)*)
(*carp1Kine=*)
(*{((Location[Point[carpus,1]]/.sol/.{T->tEval})/sL)[[1]],((Location[Point[carpus,1]]/.sol/.{T->tEval})/sL)[[2]],*)
(*((Velocity[Point[carpus,1]]/.sol/.{T->tEval})/sL * sT)[[1]],((Velocity[Point[carpus,1]]/.sol/.{T->tEval})/sL * sT)[[2]],*)
(*((Acceleration[Point[carpus,1]]/.sol/.{T->tEval})/sL * sT^2)[[1]],((Acceleration[Point[carpus,1]]/.sol/.{T->tEval})/sL * sT^2)[[2]]*)
(*};*)
(*Export[StringJoin[currPath, "/carpusP1.mat"],carp1Kine,"MAT"];*)


(* ::Input:: *)
(*(* P2 on carpus *)*)
(*carp2Kine=*)
(*{((Location[Point[carpus,2]]/.sol/.{T->tEval})/sL)[[1]],((Location[Point[carpus,2]]/.sol/.{T->tEval})/sL)[[2]],*)
(*((Velocity[Point[carpus,2]]/.sol/.{T->tEval})/sL * sT)[[1]],((Velocity[Point[carpus,2]]/.sol/.{T->tEval})/sL * sT)[[2]],*)
(*((Acceleration[Point[carpus,2]]/.sol/.{T->tEval})/sL * sT^2)[[1]],((Acceleration[Point[carpus,2]]/.sol/.{T->tEval})/sL * sT^2)[[2]]*)
(*};*)
(*Export[StringJoin[currPath, "/carpusP2.mat"],carp2Kine,"MAT"];*)


(* ::Input:: *)
(*(* P1 on ground *)*)
(*gnd1Kine=*)
(*{((Location[Point[ground,1]]/.sol/.{T->tEval})/sL)[[1]],((Location[Point[ground,1]]/.sol/.{T->tEval})/sL)[[2]],*)
(*((Velocity[Point[ground,1]]/.sol/.{T->tEval})/sL * sT)[[1]],((Velocity[Point[ground,1]]/.sol/.{T->tEval})/sL * sT)[[2]],*)
(*((Acceleration[Point[ground,1]]/.sol/.{T->tEval})/sL * sT^2)[[1]],((Acceleration[Point[ground,1]]/.sol/.{T->tEval})/sL * sT^2)[[2]]*)
(*};*)
(*Export[StringJoin[currPath, "/groundP1.mat"],gnd1Kine,"MAT"];*)


(* ::Input:: *)
(*(* P1 on mV *)*)
(*mV1Kine=*)
(*{((Location[Point[mV,1]]/.sol/.{T->tEval})/sL)[[1]],((Location[Point[mV,1]]/.sol/.{T->tEval})/sL)[[2]],*)
(*((Velocity[Point[mV,1]]/.sol/.{T->tEval})/sL * sT)[[1]],((Velocity[Point[mV,1]]/.sol/.{T->tEval})/sL * sT)[[2]],*)
(*((Acceleration[Point[mV,1]]/.sol/.{T->tEval})/sL * sT^2)[[1]],((Acceleration[Point[mV,1]]/.sol/.{T->tEval})/sL * sT^2)[[2]]*)
(*};*)
(*Export[StringJoin[currPath, "/mVP1.mat"],mV1Kine,"MAT"];*)


(* ::Input:: *)
(*(* P1 on dactyl *)*)
(*dac1Kine=*)
(*{((Location[Point[dactyl,1]]/.sol/.{T->tEval})/sL)[[1]],((Location[Point[dactyl,1]]/.sol/.{T->tEval})/sL)[[2]],*)
(*((Velocity[Point[dactyl,1]]/.sol/.{T->tEval})/sL * sT)[[1]],((Velocity[Point[dactyl,1]]/.sol/.{T->tEval})/sL * sT)[[2]],*)
(*((Acceleration[Point[dactyl,1]]/.sol/.{T->tEval})/sL * sT^2)[[1]],((Acceleration[Point[dactyl,1]]/.sol/.{T->tEval})/sL * sT^2)[[2]]*)
(*};*)
(*Export[StringJoin[currPath, "/dactylP1.mat"],dac1Kine,"MAT"];*)


(* ::Input:: *)
(*(* Moment from spring *)*)
(*springM = (springMoment*)
(*/.sol/.{T->tEval})/sM/sL^2*sT^2;*)
(*Export[StringJoin[currPath, "/springMoment.mat"],springM,"MAT"];*)


(* ::Input:: *)
(*(* Moment from drag *)*)
(*dragM = (dragMoment*)
(*/.sol/.{T->tEval}) /sM/sL^2*sT^2;*)
(*Export[StringJoin[currPath, "/dragMoment.mat"],dragM,"MAT"];*)


(* ::Input:: *)
(*(* mV angle*)*)
(*Export[StringJoin[currPath, "/mvAng.mat"],Angle[mV,1]/.sol/.{T->tEval},"MAT"];*)


(* ::Input:: *)
(*(* dactyl angle*)*)
(*Export[StringJoin[currPath, "/dacAng.mat"],Angle[mV,1]/.sol/.{T->tEval},"MAT"];*)
